%% 计算参考速度
function [ref_v] = calc_vel(ref_x, ref_y, dt)
    % 计算位置的差分
    dx = diff(ref_x);
    dy = diff(ref_y);
    % 计算速度，除以时间步长
    ref_v = sqrt(dx.^2 + dy.^2) / dt;
    % 补充第一个元素，因为差分会减少一个元素
    %ref_v = [ref_v(1), ref_v];
end